#include "vision_ruler/VisionRuler.h"

#include <assert.h>

#include <string>

#include "INIReader.h"
#include "flashai/commons/spdlog_utils.h"

vision_ruler::VisionRuler::VisionRuler() {}
vision_ruler::VisionRuler::~VisionRuler() {}

void SplitStr(const std::string& s, std::vector<std::string>& tokens,
              const std::string& delimiters = ",") {
  std::string::size_type last_pos = s.find_first_not_of(delimiters, 0);
  std::string::size_type pos = s.find_first_of(delimiters, last_pos);
  while (std::string::npos != pos || std::string::npos != last_pos) {
    tokens.push_back(s.substr(last_pos, pos - last_pos));
    last_pos = s.find_first_not_of(delimiters, pos);
    pos = s.find_first_of(delimiters, last_pos);
  }
}

void InitCameraInfoByCfg(INIReader& reader, std::string section,
                         vision_ruler::camera_info& cameraInfo) {
  assert(reader.HasSection(section));
  cameraInfo.fx = reader.GetReal(section, "fx", 0);
  cameraInfo.fy = reader.GetReal(section, "fy", 0);
  cameraInfo.cx = reader.GetReal(section, "cx", 0);
  cameraInfo.cy = reader.GetReal(section, "cy", 0);
  cameraInfo.k1 = reader.GetReal(section, "k1", 0);
  cameraInfo.k2 = reader.GetReal(section, "k2", 0);
  cameraInfo.k3 = reader.GetReal(section, "k3", 0);
  cameraInfo.p1 = reader.GetReal(section, "p1", 0);
  cameraInfo.p2 = reader.GetReal(section, "p2", 0);
  cameraInfo.width = reader.GetInteger(section, "image_width", 0);
  cameraInfo.height = reader.GetInteger(section, "image_height", 0);
}

void vision_ruler::VisionRuler::initFromConfigFile(INIReader& reader,
                                                   std::string section) {
  std::vector<std::string> tokens;

  std::string camera_info_sections_str =
      reader.GetString(section, "camera_info_sections", "");
  SplitStr(camera_info_sections_str, tokens);
  cameraInfos.resize(tokens.size());
  for (int i = 0; i < tokens.size(); i++) {
    InitCameraInfoByCfg(reader, tokens[i], cameraInfos[i]);
  }

  // parse str "class1,h1,w1,type1,class2,h2,w2,type2..."
  auto raw_str = reader.GetString(section, "class_id_to_info", "");
  tokens.clear();
  SplitStr(raw_str, tokens);
  assert(tokens.size() > 0 && tokens.size() % 4 == 0);
  for (int i = 0; i < tokens.size(); i += 4) {
    int class_id = std::atoi(tokens[i].c_str());
    float height = std::atof(tokens[i + 1].c_str());
    float width = std::atof(tokens[i + 2].c_str());
    class_id_to_shape_[class_id] = {height, width};
    class_id_to_type_[class_id] = std::atoi(tokens[i + 3].c_str());
  }

  doRectify = reader.GetInteger(section, "doRectify", 0);
  loggerLevel = reader.GetInteger(section, "loggerLevel", 0);
  if (loggerLevel == 0) {
    simple_logger::getInstance()->set_logger_Level(simple_logger::info_level);
  }
  if (loggerLevel == 1) {
    simple_logger::getInstance()->set_logger_Level(simple_logger::debug_level);
  }
}

void vision_ruler::VisionRuler::computeDistance(
    std::vector<flashai::commons::Box>& boxes, vision_ruler::global_info ginfo,
    int camera_info_id) {
  assert(camera_info_id < cameraInfos.size());

  // convert from commons box to vision ruler box
  std::vector<vision_ruler::box_info> target_boxes;
  for (int i = 0; i < boxes.size(); i++) {
    if (class_id_to_type_.count(boxes[i].class_id) == 0) continue;
    vision_ruler::box_info cur_box;
    cur_box.boxId = i;
    int cur_type = class_id_to_type_[boxes[i].class_id];
    cur_box.comefrom = cur_type;
    if (cur_type == 0) {
      // camera
      cur_box.left_top_pixel = cv::Point2f(boxes[i].x1, boxes[i].y1);
      cur_box.right_bottom_pixel = cv::Point2f(boxes[i].x2, boxes[i].y2);
    } else {
      // lidar
      cur_box.lidar_estimate_distance = boxes[i].distance;
    }
    target_boxes.push_back(cur_box);
  }

  if (doRectify) {
    distortion_Correction_Util.doCorrectPoint(target_boxes,
                                              cameraInfos[camera_info_id]);
  }

  for (size_t i = 0; i < target_boxes.size(); i++) {
    vision_ruler::box_info& box = target_boxes[i];
    auto cur_shape = class_id_to_shape_[boxes[box.boxId].class_id];
    if (box.isValid) {
      box.left_top_coordinate = cv::Point3f(0.0, 0.0, 0.0);
      box.right_top_coordinate =
          cv::Point3f(cur_shape.second * box.widthScale, 0.0, 0.0);
      box.left_bottom_coordinate =
          cv::Point3f(0.0, cur_shape.first * box.heightScale, 0.0);
      box.right_bottom_coordinate =
          cv::Point3f(cur_shape.second * box.widthScale,
                      cur_shape.first * box.heightScale, 0.0);
    }

    //   obtain3DPosition(boxes);
    projectUtil.computePose(target_boxes, cameraInfos[camera_info_id]);
    globalUtil.computePose(target_boxes, ginfo);

    // 更新原始对象中的距离数值
    for (int i = 0; i < target_boxes.size(); i++) {
      if (target_boxes[i].comefrom == 0) {
        // update distance for camera
        boxes[target_boxes[i].boxId].distance = target_boxes[i].distance;
        SPDLOG_DEBUG("box distance {}", target_boxes[i].distance);
      }
      boxes[target_boxes[i].boxId].latitude = target_boxes[i].latitude;
      boxes[target_boxes[i].boxId].longitude = target_boxes[i].longitude;
    }
  }
}

void vision_ruler::VisionRuler::obtain3DPosition(
    std::vector<vision_ruler::box_info>& boxes) {
  for (size_t i = 0; i < boxes.size(); i++) {
    vision_ruler::box_info& box = boxes[i];
    if (box.comefrom != 0) continue;
    if (box.isValid) {
      box.left_top_coordinate = cv::Point3f(0.0, 0.0, 0.0);
      box.right_top_coordinate =
          cv::Point3f(headInfo.width * box.widthScale, 0.0, 0.0);
      box.left_bottom_coordinate =
          cv::Point3f(0.0, headInfo.height * box.heightScale, 0.0);
      box.right_bottom_coordinate =
          cv::Point3f(headInfo.width * box.widthScale,
                      headInfo.height * box.heightScale, 0.0);
    }
  }
}
